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ROAD CURVATURE ESTIMATION AND AUTOMOTIVE TARGET STATE 

ESTIMATION SYSTEM 

CROSS-REFERENCE TO RELATED APPLICATIONS 
The instant application claims the benefit of prior U.S. Provisional Application Serial No. 
5 60/396,21 1 filed on July 15, 2002, which is incorporated herein by reference. 

BRIEF DESCRIPTION OF THE DRAWINGS 
In the accompanying drawings: 

FIG. 1 illustrates a block diagram of hardware associated with a predictive collision 
sensing system; 

10 FIG. 2 illustrates a coverage pattern of a radar beam used by the predictive collision 

sensing system; 

Fig. 3 depicts a driving scenario for purposes of illustrating the operation of the 
predictive collision sensing system; 

Fig. 4 illustrates a block diagram of the hardware and an associated signal processing 
15 algorithm of the predictive collision sensing system; 

Fig. 5 illustrates a flow chart of an associated signal processing algorithm of the 
predictive collision sensing system; 

Fig. 6 illustrates a geometry used for determining curvature parameters of a roadway; 

Fig. 7 illustrates the geometry of an arc; 

20 Figs. 8a-d illustrates an example of the estimation of target position, lateral velocity, 

and road curvature parameters for a straight roadway; 

Figs. 9a-b illustrate an example of the target state RMS errors from unconstrained and 
constrained filtering on the straight roadway, corresponding to Figs. 8a-d; 

Figs. lOa-d illustrate an example of the estimation of target position, lateral velocity, 
25 and road curvature parameters for a curved roadway; 

Figs, lla-b illustrate an example of the target state RMS errors fi-om unconstrained 
and constrained filtering for the curved roadway, corresponding to Figs. lOa-d; 

Figs. 12a-d illustrate an example of the estimation of target position, lateral velocity, 
and associated RMS errors for a straight roadway involving a lane change; and 
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Figs. 13a-d illustrates an example of the estimation of target position, lateral velocity, 
and their RMS errors for a curved roadway involving a lane change. 

DESCRIPTION OF EMBODIMENT(S) 
Referring to Fig, 1, a predictive collision sensing system 10 incorporated in a host 

5 vehicle 12, comprises a radar system 14 for sensing objects external to the host vehicle 12, 
and a set of sensors, including a yaw rate sensor 16, e.g. a gyroscopic sensor, and a speed 
sensor 18, for sensing motion of the host vehicle 12. The yaw rate sensor 16 and speed 
sensor 18 respectively provide measurements of the yaw rate and speed of the host vehicle 
12. The radar system 14, e.g. a Doppler radar system, comprises an antenna 20 and a 

10 radar processor 22, wherein the radar processor 22 generates the RF signal which is 
transmitted by the antenna 20 and which is reflected by objects in view thereof The radar 
processor 22 demodulates the associated reflected RF signal that is received by the antenna 
20, and detects a signal that is responsive to one or more objects that are irradiated by the RF 
signal transmitted by the antenna 20. For example, the radar system 14 provides target 

15 range, range rate and azimuth angle measurements in host vehicle 12 fixed coordinates. 
Referring to Fig. 2, the antenna 20 is adapted to generate a radar beam 23 of RF energy that 
is, for example, either electronically or mechanically scanned across an azimuth range, e.g. 
+/- Y5 e.g. +/- 50 degrees, responsive to a beam control element 24, and which has a distance 
range, e.g. about 100 meters, fi-om the host vehicle 12 that is sufficiently far to enable a 

20 target to be detected sufficiently far in advance of a prospective collision with the host 
vehicle 12 so as to enable a potentially mitigating action to be taken by the host vehicle 12 so 
as to either avoid the prospective collision or mitigate damage or injury as a result thereof. 
The radar processor 22, yaw rate sensor 16, and speed sensor 18 are operatively connected 
to a signal processor 26 that operates in accordance with an associated predictive collision 

25 sensing algorithm to determine whether or not a collision with an object, e.g. a target vehicle 
36 (illustrated in Fig. 3), is likely, and if so, to also determine an action to be taken 
responsive thereto, for example, one or more of activating an associated warning system 28 
or safety system 30 (e.g. frontal air bag system), or using a vehicle control system 32 (e.g. 
an associated braking or steering system) to take evasive action so as to either avoid the 

30 prospective collision or to reduce the consequences thereof. 

Referring to Fig. 3, the host vehicle 12 is shown moving along a multiple lane 
roadway 34, either straight or curved, and there is also shown a target vehicle 36 moving in 
an opposite direction, towards the host vehicle 12, Generally, there can be any number of 
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tai^et vehicles 36 that can fit on the roadway 34, each moving in the same or opposite 
direction as the host vehicle 12. These target vehicles 36 can either be in the host lane 38 or 
in a neighboring lane 40 either adjacent to or separated from the host lane 38, but generally 
parallel thereto. For purposes of analysis, it is assumed that the host vehicle 12 moves along 
5 the center line 41 of its lane 38 steadily without in-lane wandering, and the road curvatures 
of all the parallel lanes 38, 40 are the same. Road curvatvire is assumed small such that the 
differences between the heading angles of the host vehicle 12 and any detectable target 
vehicles 36 are smaller than 15 degrees. 

Referring to Fig. 4, the predictive collision sensing system 10 uses the 
10 measurements of speed and yaw rate of the host vehicle 12 from the speed sensor 18 
and the yaw rate sensor 16 respectively therein; and the measurements of target range r, 
range rate r and azimuth angle r| for all target vehicles 36 from the radar system 14 
mounted on the host vehicle 12; along with the corresponding error covariance matrices of 
all these measurements, to estimate each target's two dimensional position, velocity and 

15 acceleration jc,>',>^,3>] in the host fixed coordinate system at every sampling instance, 
preferably with an error as small as possible. The predictive collision sensing system 10 
comprises 1) a road curvature estimation subsystem 42 for estimating the curvature of the 
roadway 34 using measurements from the host vehicle motion sensors, i.e. the yaw rate 
sensor 16 and speed sensor 18; 2) an unconstrained target state estimation subsystem 44 

20 for estimating the state of a target illuminated by the radar beam 23 and detected by the 
radar processor 22; 3) a constrained target state estimation subsystem 46 for estimating 
the state of the constraint on the target, assuming that the target is constrained to be on the 
roadway 34, either in the host lane 38 or in a neighboring lane 40, for each possible lane 
38, 40; 4) a target state decision subsystem 48 for determining whether the best estimate of 

25 the target state is either the unconstrained target state, or a target state constrained by one of 
the constraints; and 5) a target state fusion subsystem 50 for fusing the unconstrained target 
state estimate with the appropriate constraint identified by the target state decision 
subsystem 48 so as to generate a fused target state. The best estimate of target state - either 
the unconstrained target state or the fused target state — is then used by a decision or control 

30 subsystem for determining whether or not the host vehicle 12 is at risk of collision with the 
target, and if so, for determining and effecting what the best course of action is to mitigate the 
consequences thereof, e.g. by action of either the warning system 28, the safety system 30, 
or the vehicle control system 32, or some combination thereof. When possible, the use of 
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the geometric structure of the roadway 34 as a constraint to the target kinematics provides 
for a more accurate estimate of the target state, which thereby improves the reliabiUty of any 
actions taken responsive thereto. 

Referring also to Fig. 5, illustrating a method 500 of detecting the state, i.e. kinematic 
5 state variables, of a target in view of the host vehicle 12, the steps of which are, for example, 
carried out by the signal processor 26, in steps (502) and (504), the speed and yaw rate 
of the host vehicle 12 relative to the roadway 34 are respectively read from the speed 
sensor 18 and the yaw rate sensor 16 respectively. Then, in step (506), the curvature 
parameters and associated covariance thereof of the roadway 34 are estimated using first 52 
10 and second 54 Kalman filters that respectively estimate the state (i.e. kinematic state 
variables of the host vehicle 12) and associated covariance thereof of the host vehicle 12, 
and then the curvature parameters and associated covariance thereof of the roadway 34, as 
described hereinbelow, wherein the curvature parameters and associated covariance thereof 
of the roadway 34 are then subsequently used by the constrained target state estimation 
15 subsystem 46 to generate associated constraints on the possible location of a prospective 
target vehicle 36. 

A well-designed and constructed roadway 34 can be described by a set of parameters, 
including curvature, wherein the curvature of a segment of the roadway 34 is defined as: 

« (1) 

20 where R is the radius of the segment. In general, for a piece of smooth roadway 34, the 
curvature variation can be described as a function of a distance / along the roadway 34 by a 
so-called clothoid model, i.e.: 




(2) 



25 



Referring to Fig. 6, the heading angle ^defining the heading direction is given by: 




(3) 



Substituting equation (2) into equation (3) gives 



Ae = e-e^=c^i+c^i^ 12 



(4) 
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Referring to Fig. 6, the equation of the roadway 34, i.e. the road equation, in x-y 
coordinates is given by: 

X = + |cos^(T)(ix (5) 
and . 

y^y^-^[sm0(x)dT'. rj; (6) 

Assuming the heading angle ^to be within 15 degrees, i.e. |0 1<15'*, equations (5) and 
(6) can be approximated by: 

Ax = X - jCq » / (7) 

Ax^ _ Ax^ 



2^6 (8) 
10 Accordingly, the roadway 34 is modeled by an incremental road equation in terms of 

curvature coefficients: Co and Cj. This incremental road equation describes a broad range of 
road shapes as follows: 1) Straight roadway 34: Co=0 and Ci=0; 2) circular roadway 34: 
Ci—0; and 3) a general roadway 34 with an arbitrary shape for which the change in heading 
angle ^is less than 15 degrees: Co>0. 

15 The road curvature parameters Q and are estimated using data from motion 

sensors (yaw rate sensor 16 and speed sensor 18) in the host vehicle 12, based upon the 
assvimption that the host vehicle 12 moves along the center line 41 of the roadway 34 or 
associated host lane 38. 

The road curvature parameters and C, can be calculated from data of co, d> ^U, U 

20 responsive to measurements of yaw rate a}^ and speed l/ of the host vehicle 12 from the 
available host vehicle 12 motion sensors. However, generally the measurements of yaw rate 
gP and speed f/, from the yaw rate sensor 16 and speed sensor 18 respectively, are noisy. 
A host state filter implemented by a first Kalman filter 52 is beneficial to generate estimates 
of , ?7, C/ from the associated noisy measurements of yaw rate aP and speed lf*\ after 

25 which a curvature filter implemented by a second Kalman filter 54 is used to generate 
smoothed estimates of the curvature parameters Cq and Cj . The dynamics of the host 

vehicle 12 for the host state filter follows a predefined set of kinematic equations (constant 
velocity in this case) given by: 
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10 



15 



20 



25 



where 



1 r 0 0 

0 10 0 

0 0 1 r 

0 0 0 1 



v:~iV(0,R*) 



(9) 
(10) 



10 0 0 
0 0 10 



u 
u 

CO 



and 



U" 



CO 



(11) 



and where T is the samplmg period, superscript (•) is used to indicate that the filter is the host 
fiher, and if and <^ are host vehicle 12 speed and yaw rate measurements. The first 
Kalman filter 52 is implemented to estimate the host state xj^j^^ and its error covariance P*|j^ , 
as illustrated in Fig. 4. 

The estimate of the host state from the first Kalman filter 52, i.e. the host state filter, 
is then used to generate a synthetic measurement that is input to the second Kalman filter 
54, i.e. curvature coefficient filter, wherein the associated Kalman filters 52, 54 operate in 
accordance with the Kalman filtering process described more fiiUy in the Appendix 
hereinbelow. The relationship between the road curvature parameters Cq , Q and the host 

state variables o), d)^U,U is derived as follows: 

From equation (4), the radius R of road curvature is expressed generally as a function 
R(l) of the distance / along the roadway, as is illustrated in Fig. 7. Taking the time derivative 
on both sides of equation (4) yields: 

^ = Q ./ +C, ./ ./ = (Co +C, •/) •/ . (12) 

Noting that 0— (o^ the yaw rate of the host vehicle 12, and that / =C/, the speed of the 
host vehicle 12, and substituting the clothoid model of equation (2) in equation (12), yields: 

(o^C'U (13) 

or 



_ CO 

C = — . 
U 

Clothoid parameter Co is given as the value of curvature C at /=0, or 



(14) 



(15) 
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Taking the derivative on both sides of equation (14) yields 



15 



(16) 



U C/^ 

Using the definition of Ci, from equation (2), C\ may be expressed in temis of the 
host state as follows: 



dl~ dt' dl" U~ 



(17) 



The system equations for the second Kalman filter 54, i.e. the curvature filter, that 
generates curvatvire estimates C^^^^ and Cj^^^ , are given by 



10 v\^here 



1 AtU 
0 1 



1 0 
0 1 



c 

= 



(18) 
(19) 

(19a) 



is the update time period of the second Kalman filter 54, and the values of the elements 
of the measurement vector are given by the corresponding values of the state variables ~ 
i.e. the clothoid parameters Co and Ci ~ of the curvature filter. 



The measurement, Zj^ , is transformed from the estimated state [ , c7 , fi? , d)]k as 



follows: 



CO 

U 



(20) 



k\k 



and the associated covariance of the measurements is given by: 



R* =J*P*i*(J*)'' 



(21) 



20 where 



lo) 2>d)U 



I 0 

u 



(22) 



k\k 
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It should be understood that other systems and methods for estimating the curvature 
parameters of the roadway 34 may be substituted in the road curvature estimation 
subsystem 42 for that described above. For example, the curvature parameters of the 
roadway may also be estimated from images of the roadway 34 by a vision system, either 

5 instead of or in conjunction with the above described system based upon measurements of 
speed f/* and yaw rate c/* from associated motion sensors. Furthermore, it should be 
understood that yaw rate can be either measured or determined in a variety of ways, or using 
a variety of means, for example, but not limited to, using a yaw gyro sensor, a steering angle 
sensor, a differential wheel speed sensor, or a GPS-based sensor; a combination thereof; or 

10 functions of measurements therefrom (e.g. a function of, inter alia, steering angle rate). 

Referring again to Fig. 5, in step (508), the measurements of target range r, range rate 
r , and azimuth angle ti are read from the radar processor 22, and are used as inputs to an 
extended Kalman filter 56, i.e. the main filter, which, in step (510), generates estimates of 
the unconstrained target state — i.e. the kinematic state variables of the target - which 

15 estimates are relative values in the local coordinate system of the host vehicle 12 (i.e. the 
host-fixed coordinate system) which moves wdth therewdth. In step (512), the unconstrained 
target state, i.e. the target velocity and acceleration, is transformed to absolute coordinates of 
the absolute coordinate system fixed on the host vehicle 12 at the current instant of time as 
illustrated in Fig. 3, so as to be consistent with the absolute coordinate system in which the 

20 road constraint equations are derived and for which the associated curvature parameters are 
assumed to be constant, when used in the associated constraint equations described 
hereinbelow in order to generate estimates of the constrained target state. The absolute 
coordinate system superimposes the moving coordinate system in space at the current instant, 
so that the transformation in step (512) is realized by adding the velocities and accelerations 

25 of the host vehicle 12 to the corresponding target estimates, in both jc and 3/ directions. 

The result from the coordinate transformation in step (512) of the output from the 
extended Kalman filter 56 is then partitioned into the following parts, corresponding 
respectively to the x and y position of the target vehicle 36 relative to the host vehicle 12, 
wherein the superscript 1 refers to the unconstrained target state of the target vehicle 36: 



30 i\ = 

— 



^1 



and P/ = 



P P' 

pi pi 



(23) 
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Referring again to Fig. 5, foUovsdng steps (506) and (512), in steps (514) through 
(524) described more fully hereinbelow, various constraints on the possible trajectory of the 
target vehicle 36 are applied and tested to determine if the tai^et vehicle 36 is likely 
traveling in accordance with one of the possible constraints. For example, the constraints are 

5 assumed to be from a set of lanes that includes the host lane 38 and possible neighboring 
lanes 40, and a target vehicle 36 that is likely traveling in accordance with one of the 
possible constraints would likely be traveling on either the host lane 38 or one of the possible 
neighboring lanes 40. In step (524), the hypothesis that the target vehicle 36 is traveling on 
either the host lane 38 or one of the possible neighboring lanes 40 is tested for each possible 

10 lane. If the hypothesis is not satisfied for one of the possible lanes, then, in step (526), the 
state of the target is assumed to be the unconstrained target state, which is then used for 
subsequent predictive crash sensing analysis and control responsive thereto. Otherwise, from 
step (524), in step (528), the target state is calculated by the target state fusion subsystem 
50 as the fusion of the unconstrained target state with the associated state of the constraint 

15 that was identified in step (524) as being most likely. 

Prior to discussing the process of steps (514) through (524) for determining whether 
the target is likely constrained by a constraint, and if so, what is the most likely constraint, the 
process of fusing the unconstrained target state with state of a constraint will first be 
described for the case of a target vehicle 36 moving in the same lane as the host vehicle 12. 

20 The constraints are assumed to be active in y-direction only, consistent with the assumptions 
that the host vehicle 12 moves along the center line 41 of its lane 38 steadily without in-lane 
wandering and that the road curvatures of all the parallel lanes 38, 40 are the same, and 
given that the absolute coordinate system is fixed on the host vehicle 12 at the current instant 
of time. Assuming the target vehicle 36 is moving in the same lane 38 as the host vehicle 

25 12, and using the road constraint equation with the estimated coefficients, in step (514), the 
constraint state variables are then given in terms of the lateral kinematic variable as: 




y 



y 



(24) 



and 



(25) 



30 



where 
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15 



20 



and 



CpJc' +C,(x')V2 0 0 

CoJfc' + C^x'i' Cox' + C, (i' y /2 0 

Co* ' + C, (x' )^ + ' 2Coi + 2C, Jt ' + C, (ic' )^ /l 



(xy/2 



(26) 



-1 "1 



(xy/6 

(x'f-i'/2 . (27) 
(jt*)^+jc'-^' jc'-(jc')^+(jc')^-x72 

In step (528), the two >*-coordinate estimates, one from the main fiher and the other 
from the road constraint, are then fused as follows: 



Finally, the composed estimate of the target state is 



(28) 
(29) 



^1 



(30) 



10 and 



(31) 



where 



(32) 



In step (530), this composed estimate would then be output as the estimate of the 
target state if the target vehicle 36 were to be determined from steps (514) through (524) to 
be traveling in the host lane 38. 

Retuming to the process of steps (514) through (524) for determining whether the 
target is Hkely constrained by a constraint, and if so, what is the most likely constraint; 
according to the assumption that targets follow the same roadway 34, if the target vehicle 36 
were known to travel in a particular lane, it would desirable to use estimated road parameters 
for that lane as a constraint in the main filter of estimating target kinematics. However, the 
knowledge of which leme the target vehicle 36 is current in is generally not available. 
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especially when the target is moving on a curved roadway 34. Since the road equation (8) is 
only for the host lane 38 in the host-centered coordinate system, constrained filtering would 
require knovsdng which lane the target is in, and different constraint equations would be 
needed for different lanes. Ignoring the difference of road curvature parameters among these 
parallel lanes, i.e. assuming the curvature of each lane to be the same, the road equation for 
an arbitrary lane can be written as: 



where B is the width of the lanes and m represents the lane to be described (w = 0 
corresponds the host lane 38, /w = 1 corresponds the right neighboring lane 40, w = -1 
corresponds the left neighboring lane 40, and so on). Without the prior knowledge of the 
target lane position, each of the multiple constraints forming a multiple constraint system 
(analogous to the so-called multiple model system) is tested to determine identify which, if 
any, of the constraints are active. A multiple constraint (MC) system is subjected to one of a 
finite number ISF of constraints. Only one constraint can be in effect at any given time. Such 
systems are referred to as hybrid ~ they have both continuous (noise) state variables as well 
as discrete number of constraints. 

The following definitions and modeling assumptions are made to facilitate the 
solution of this problem: 

Constraint equations: 



where f ^ denotes the constraint at time tk in effect during the sampling period ending 

at /it. 




2 



(33) 



(34) 



Constraint: among the possible constraints 




(35) 




: state estimate at time tk using constreiint / 



9 ' CO variance matrix at time tk under constraint 



ju/^ ^ : probability that the target is following constraint j at time /^.y 
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Constraint jump process: is a Markov chain with known transition probabilities 

^k.=/i/...=/'}=^(.- (36) 
To implement the Markov model - for systems with more than one possible constraint 
state — it is assumed that at each scan time there is a probability py that the target will make 
the transition from constraint state / to state y. These probabilities are assumed to be known a 
priori and can be expressed in the probability transition matrix as shown below. 

New State 
1 2 3 

ll"/?!, Pn Pn 

Pix Pn Pi3 (37) 
_P3\ Pyi /^33^ 

The prior probability that is correct ( is in effect) is 



Pr/or 
P = 2 
'"^ State 

3 



p{f!\z') = ^ll y = i,-,iv^ (38) 

where 2^ is the prior information and 

ILK^"^ (39) 
since the correct constraint is among the assumed nF possible constraints. 

The constrained target state estimation subsystem 46 provides for determining 
whether the target state corresponds to a possible constrained state, and if so, then provides 
for determining the most likely constrained state. 

One way of determining this is the multiple model filtering algorithm proposed by 
Bar-Shalom, wherein IsF parallel filters are run simultaneously in parallel. 

In another way, a multiple constraint (MC) estimation algorithm mixes and updates 
1<F constraint-conditioned state estimates using the unconstrained state estimate y as a 

measurement, along with the calculation of the likelihood function and probability associated 
with each constraint. In one embodiment of the multiple constraint (MC) estimation 
algorithm, the constrained state estimate output is a composite combination of all of the 
constraint-conditioned state estimates. If this constrained state estimate is valid, i.e. if the 
constrained state estimate corresponds to the unconstrained state estimate, then the target 
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15 



20 



State is given by fusing the constrained and unconstrained state estimates; otherwise the target 
state is given by the unconstrained state estimate. This embodiment of the multiple constraint 
(MC) estimation algorithm comprises the following steps: 

1« Estimation of state variables from multiple constraints; In step (514), using the 
multiple lane road equation (33) to replace the first row in equation (24), the multiple 
constraint state estimates are given by: 

















y 











5,+Co.(ic')V2 + C,-(x')V6 
Co-x'-i'+C,-(ic')^.i72 
Co iiy +Co • jc' -^^ +C, -x' 'i^y +C, -ix'f -^72 



(40) 



where Bj = 0,±B, ± 



B , and B is the width of a lane. Stated in another way, the 



constraint state estimates correspond to the y locations of the centerlines of each possible lane 
in which the target vehicle 36 could be located. 

The associated coveiriance is given by: 

= Ai -P,,^ -iKf + -P^t -(A^)^ (41) 
where and are given by equation (26) and equation (27), Pj,, is from equation (23) 
and Pj^j^ is from the curvature filter. 

2, Constraint-conditioned updating: In step (516), the state estimates and 
covariance conditioned on a constraint being in effect are updated, as well as the constraint 
likelihood function, for each of the constraints y = 1, ... iV^. The updated state estimate and 

covariances corresponding to constraint j are obtained using measurement j)' , as follows: 



+p;^ (p;> +p; V'fj)' -r l 



y, =y, 



^ ' fp°^ +p^ V^p**-' 

■ >"*i* yh\k yfk\k \ yfk\k yfk\k / >**i» " 



p > = p ^-^ — p ^-^ 

p7 =p07 _p07 ipoj pi r^poy 
* ^k\k * ^k\k ^ yh\k \ y^k\k yfk\k I ^h\k 

3. Likelihood calculation: In step (518), the likelihood function corresponding to 



(42) 

(43) 
(44) 



constraint j is evaluated at the value y of the unconstrained target state estimate, assuming a 
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Gaussian distribution of the measurement around the constraint-conditioned state estimate for 
each of the constraints y = 1 , . . . iV^, as follows: 

A-; =N(y' ; , P"-^ 1 (45) 

wherein the Gaussian distribution N( ; , ) has a mean value of y and an associated 
covariance of P^^ + Pi . 

4. Constraint probability evaluations; In step (520), the updated constraint 
probabilities are calculated for each of the constraints 7=1,... N^, as follows: 

Mi=^Maj (46) 
where Oj , the probability after transition that constraint J is in effect, is given by 

^j=f.Po-K. (47) 

1=1 

and the normalizing constant is 

^ = Z^i^y (48) 

5. Overall state estimate and covariance : In step (522), the combination of the 
latest constraint-conditioned state estimates and covariances is given by: 



10 



7=1 



(50) 



p^^-ZK-K^ (51) 

The output of the estimator from step (522) in the above algorithm is then used as the 
constrained estimates in the fusion process described by equations (28) and (29), and the 
20 result of equation (51), instead of the result of equation (32), is used in equation (31). 

When the target vehicle 36 is not following the roadway 34 or is changing lanes, 
imposing the road constraint on target kinematic state variables will result in incorrect 
estimates that would be worse than using the associated unconstrained estimates. However, 
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noise related estimation errors might cause a correct road constraint to appear invalid. 
Accordingly, it is beneficial to incorporate a means that can keep the constraints in effect 
when they are valid, e.g. when the target vehicle 36 follows a particular lane; and lift them 
off promptly when they are invalid, e.g. when the tai^et vehicle 36 departs from its lane. 
The unconstrained target state estimate plays a useful role in road constraint validation, since 
it provides independent target state estimates. 

One approach is to test the hypothesis that the unconstrained target state estimate 
satisfies the road constraint equation, or equivalently, that the constrained estimate and the 
unconstrained estimate each correspond to the same target. The optimal test would require 
using all available target state estimates in history through time tk and is generally not 
practical. A practical approach is the sequential hypothesis testing in which the test is carried 
out based on the most recent state estimates only. In accordance with the notation used 
hereinabove, the difference between the constrained and unconstrained target state estimates 
(y direction only) is denoted: 

4=J^; ^y, (52) 

as the estimate of 




=0 



(54) 



vs. 



S, ^0 

— 'k 



(55) 



The main filter error 



y, =y, -y, 



h\k 



(56) 



is assumed independent of the error 



y, =y, -y, 



(57) 



which is fix)m the constraints. The covariance of the difference S_,^ is, under hypothesis Ho, 
given by: 
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(58) 




Assuming that the estimation errors are Gaussian, the test of Hq vs. H\ is as follows: 



Accept Ho if A. ^t.MYL 



(59) 



The threshold is chosen such that 



a 



(60) 



where a is a predefined error tolerance value. Note that based on the above Gaussian error 
assumption, p^^ has a chi-square distribution with Hy degrees of freedom. The choice of this 

threshold is a significant design factor and should be based on specific application need. In 
road vehicle collision prediction, a target in the host lane 38 is regarded to be on a collision 
course and is considered more dangerous than a target in one of the neighboring lanes 40. 
Thus it is desirable to have a high threshold (a low error tolerance value) for a target in host 
lane 38 since constrained filtering can provide accurate target state estimates while a 
"changing lane" maneuver of such a target will not pose a threat to the host vehicle 12. On 
the other hand, targets in neighboring lanes 40 are usually regarded as passing-by vehicles. 
Though constrained filtering may further reduce false alarm rate, a "changing lane" maneuver 
of such a target (into the host lane 38) would pose a real threat to the host vehicle 12. Thus it 
is desirable to have a low threshold (a high error tolerance value) for a target in a neighboring 
lane if false alarm rate is already low enough. 

Based on the above analysis, the hypothesis testing scheme efficiently uses different 
threshold values for targets in different lanes, with the multiple constraint filtering algorithm 
providing the knowledge of which lane the target is most likely in currently. Assuming that 
there are possible lanes on the roadway 34, and each lane is described by a constraint 
equation, the constraint equation with the highest probability for a target corresponds to 

the lane that this target in most likely in at time tk (the current time). Denoting this most likely 
lane as //, then 



The difference between the unconstrained state estimates and lane // constrained state 
estimates (y direction only), denoted as: 




(61) 
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(62) 



(63) 



where is the true target state and y^^ is the true state of a target moving along lane //. 
The test for the "same target" hypothesis is then given by: 

(64) 



vs. 



The constrained estimation error is given by: 



(65) 



(66) 



Assuming that the estimation errors are independent and Gaussian, the test of Hq vs. 
H\ becomes: 



Accept /fo if p',; =(i;;)'(P,f m <r,, 



(67) 



where 



P,*"' = E 



(^i-^;:X^i-i;:)'l=4fe:-2;:k-2;;)' 



(68) 



_ pi ^ p/, 
and the threshold is such that 

P(p': >r,.\Ho>h)=cc,, 

where 



(69) 



n^Vj]]., and e^J;^, (70) 
Such a lane adaptive hypothesis testing scheme provides for a prompt switch of the 
target state estimation output to the unconstrained estimate when the target vehicle 36 leaves 
its current lane, while the estimation accuracy of a target in host lane 38 is substantially 
improved by constrained filtering. 
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In another embodiment of the multiple constraint (MC) estimation algorithm, the 
constrained state estimate used for the hypothesis testing is the most likely of the separate 
constrained target state estimates (i.e. in accordance with a 'Svinner take all" strategy), rather 
than a composite combination of all of the constrained target state estimates. If this most 
5 likely constrained state estimate is valid, i.e. if the most likely constrained state estimate 
corresponds to the unconstrained state estimate, then the target state is given by fusing the 
most likely constrained state estimate and the imconstrained state estimate; otherwise the 
target state is given by the unconstrained state estimate. 

In yet another embodiment of the multiple constraint (MC) estimation algorithm, 
10 hypothesis tests are made for each of the constrained state estimates. If none of the 
hypotheses are satisfied, then the target state is given by the unconstrained state estimate. If 
one of the hypotheses is satisfied, then the target state is given by fusing the corresponding 
constrained state estimate and the unconstrained state estimate. If more than one hypotheses 
are satisfied, then the most likely constrained state may be identified by voting results firom a 
15 plvirality of approaches, or by repeating the hypothesis tests with different associated 
thresholds. 

Generally, the number of constraints (i.e. the number of roadway lanes) can vary with 
respect to time, as can associated parameters therewith, for example, the width of the lanes of 
the roadway, so as to accommodate changes in the environment of the host vehicle 12. For 
20 example, the host vehicle 12 in one trip could travel on a one-lane road, a two-lane road with 
opposing traffic, a three-lane road with a center turn lane, a four line road two lanes of 
opposing traffic, or on a multi-lane divided freeway. 

Road vehicle tracking simulations using constrained and unconstrained filtering were 
carried out for four scenarios. In all scenarios, the host vehicle 12 was moving at 15,5 m/s 

25 and a target vehicle 36 is approaching on the same roadway 34 at a speed of 15.5 m/s. The 
initial position of the target was 125 meters away from the host in the x direction, and the 
lane width for all lanes was assumed to be 3.6 meters. The measurement variance of the 
vehicle speed sensor was 0.02 m/s and the variance of the gyroscope yaw rate measurement 
was 0.0063 rad/s. The variances of radar range, range rate and azimuth angle measurements 

30 were 0.5 m, 1 m/s, and 1.5° respectively. Simulation results were then generated from 100 
Monte-Carlo runs of the associated tracking filters. 
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In the first scenario, the host vehicle 12 and the target vehicle 36 were moving on a 
straight roadway 34 (Co 0 and Ci = 0) and the target vehicle 36 was moving toward the 
host vehicle 12 in the same lane. Figs. 8a-d illustrate the target state estimation and road 
curvature estimation results of the unconstrained and constrained filtering schemes, and Fig. 
5 9a-b illustrate the average target vehicle 36 lateral position, velocity and acceleration RMS 
errors of the unconstrained and constrained filtering schemes. The estimation errors from 
constrained filtering were substantially reduced. Before 48 radar scans, when the target 
vehicle 36 was farther than 65 meters away fi-om the host vehicle 12, constrained filtering 
resulted in a more than 40 percent reduction of error in target lateral velocity estimation, and 
10 a more than 60 percent reduction of error in lateral acceleration estimation. When the target 
vehicle 36 was less than 65 meters away from the host vehicle 12, which is a more relevant 
condition for collision prediction, more than 50 percent of lateral position estimation error, 
and more than 90 percent of lateral velocity and acceleration estimation errors, were reduced 
by constrained filtering. 

15 In the second scenario, the host vehicle 12 and the target vehicle 36 were moving on 

a curved roadway 34 (Co = -10"^ and C\ = -3x10"^) and the target vehicle 36 was moving 
toward the host vehicle 12 in the same lane. Figs. lOa-d illustrate the target state estimation 
and curvature estimation results of the unconstrained and constrained filtering schemes, and 
Figs, lla-b illustrate the average target vehicle 36 lateral position, velocity and acceleration 

20 RMS errors of the unconstrained and constrained filtering schemes. The estimation errors 
from constrained filtering were substantially reduced after about 48 radar scans, when the 
target vehicle 36 was less than 65 meters away from the host vehicle 12. Estimation errors 
were the same for constrained and unconstrained filtering before 20 radar scans, when the 
target vehicle 36 was about 100 meters away from the host vehicle 12. For the target 

25 vehicle 36 located between 100 and 65 meters away from the host vehicle 12, constrained 
filtering resulted in about a 30 percent reduction in errors of latereil velocity and acceleration 
estimation, and when the target vehicle 36 was less than 65 meters away from the host 
vehicle 12, more than 50 percent of lateral position estimation error and more than 90 percent 
of lateral velocity and acceleration estimation errors were reduced by constrained filtering. 

30 The lack of improvement for constrained filtering when the target vehicle 36 was far away 
resulted from estimation errors of road curvature parameters, which caused constraint errors 
proportional to the distance between host vehicle 12 and the target vehicle 36. This is more 
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evident in the curved roadway 34 case, where curvature estimation error was larger and 
caused more lane position ambiguity of a distant target vehicle 36. 

In the third scenario, the host vehicle 12 and the target vehicle 36 were moving on a 
straight roadway 34 (Co = 0 and d = 0) and the target vehicle 36 was initially approaching 
5 in the left neighboring lane. At / = 2.2 second (55 radar scans), the target vehicle 36 began to 
diverge from its lane and turns toward the host lane 38, which resulted in a collision at / = 4 
seconds (100 radar scans). Figs. 12a-d illustrate the target state estimation results and the 
lateral position and velocity RMS errors of the unconstrained and constrained filtering 
schemes. The error tolerance levels for constraint validity hypothesis testing (equation (69)) 

10 were chosen as a « 1 for the host lane 38 and a = 0.5 for all neighboring lanes 40. Whereas 
constrained filtering without validation produces substantially lower estimation errors before 
the target vehicle 36 turns away, the associated target state estimation result was incorrect 
and its RMS errors were much larger than that of unconstrained filtering after the target 
vehicle 36 began to turn away from its lane (the left neighboring lane), implying that the road 

15 constraints, which become invalid after the target vehicle 36 began to diverge from its lane, 
were not promptly lifted off. On the other hand, the performance of constrained filtering with 
validation was substantially close to that of imconstrained filtering, producing slightly lower 
estimation errors before the target vehicle 36 turns away, and exhibiting target state 
estimation results and RMS errors that were the same as unconstrained filtering after the 

20 target vehicle 36 began to turn away from its lane, implying that road constraints were 
promptly lifted off after the target vehicle 36 began to diverge from its lane. 

The fourth scenario was similar to the third scenario, the only difference being that 
the vehicles were on a curved roadway 34 (Co = -10"^ and Ci = -3x10"^) instead of a straight 
one. The target vehicle 36 began to diverge at / = 2.2 s and results in a collision at / = 4 s. 

25 Figs. 13a-d illustrate the target state estimation results and the lateral position and velocity 
RMS errors of the unconstrained and constrained filtering schemes. The error tolerance levels 
were the same as in the third scenario, and the results and observations were also similar to 
that of the third scenario. Road constraints were promptly lifted off by the proposed 
constraint validation after the target vehicle 36 began to diverge from its lane. In general, 

30 the overall improvement by constrained filtering in estimation accuracy of target vehicle 36 
lateral kinematics was substantial, given the fact that estimation accuracy of target vehicle 
36 lateral kinematics was often limited by poor radar angular resolution. 
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Accordingly, simulation results of road vehicle tracking on both straight and curved 
roadways 34 show that the predictive collision sensing system 10 could substantially 
reduce the estimation errors in target vehicle 36 lateral kinematics when the target vehicles 
36 were in the host lane 38. When a target vehicle 36 maneuvers from a neighboring lane 
5 into the host lane 38, the predictive collision sensing system 10 promptly detects this 
maneuver and lifts off the road constraint to avoid an otherwise incorrect constrained result. 
In view of the fact that poor radar angular resolution often results in poor lateral kinematics 
estimation, the predictive collision sensing system 10 has provided for a substantial 
improvement in estimation accuracy of target vehicle 36 lateral kinematics, which is 
10 beneficial for an early and reliable road vehicle collision prediction. 

While specific embodiments have been described in detail in the foregoing detailed 
description and illustrated in the accompanying drawings, those with ordinary skill in the art 
will appreciate that various modifications and altematives to those details could be developed 
in light of the overall teachings of the disclosure. Accordingly, the particular arrangements 
15 disclosed are meant to be illustrative only and not limiting as to the scope of the invention, 
which is to be given the fiiU breadth of the appended claims and any and all equivalents 
thereof. 

APPENDIX - DESCRIPTION OF KALMAN FILTERING 

A Kalman filter is used to estimate, fi-om a set of noisy measurements, the state and 
20 associated measurements of a dynamic system subject to noise. 

The system dynamics are defined by: 

=F,-x,+>v„ w,-NiO,Q,) (A-1) 
where is the system state vector, is the system matrix and an associated vector of 

noise variables corresponding to each state variable, each noise variable having a mean value 
25 of zero, and a variance given by the corresponding element of the associated variance vector, 

Q* 

The dynamics of the associated system measurements are given by: 

z, = H, + V,, v,^N(0,K,) (A-2) 
where Zj^ is the system measurement vector, is the measurement matrix and an 
30 associated vector of noise variables corresponding to each measurement variable, each noise 
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variable having a mean value of zero, and a variance given by the corresponding element of 
the associated variance vector, Rj^ . The values of the elements of the associated covariance 

matrix Rj^ can be determined a priori from analysis of the representative measurements of 
the associated system for associated representative sets of operating conditions. The values 
of the elements of the associated covariance matrix Qj^ account for modeling errors. 
Generally, the associated matrices , Q^^ , , Rj^ can vary over time. 

Given a measurement at time k, and initial values of the state 2Eit-i|*-.i 
associated covariance P*.,,*., at time k-1, the Kalman filter is used to to estimate the 
associated state x^ia and associated covariance P^,;^ ^^^^ 

The first step in the filtering process is to calculate estimates of the state and 
associated covariance PjtjA_i at time k based upon estimates at time k-1, as follows: 

^k\k-i = P* ' ^*-i|jfc-i (A-3) 
P*,*-,=F*-P*-il*-rF7+Q* (A-4) 

The next step is to predict the measurement Zj^ and associated covariance matrix Sj^ 
at time k, as follows: 

ik^^k'^kik^i (A-5) 
S, = cov(i,) = H, .P,,,.j H] +R, (A.6) 
The next step is to calculate a geiin matrix Gj^ used for updating the state vector x^i* 
and associated covariance matrix P^^i^^ , as follows: 

G,=P,„_,-Hl.S;^' (A-7) 
Finally, the state vector x^^ and associated covariance matrix P^^ are estimated at 
time k, responsive to the associated measurement , as follows: 

= £jn*-i + Gi • (Zi - ) (A-8) 
P*^i=P*i*-.-G,-S,.Gl (A-9) 

We claim: 
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